package com.gitee.wsl.unit.angle.ext

import com.gitee.wsl.ext.bitcode.umod
import com.gitee.wsl.mathematics.interpolator.ext.FULL
import com.gitee.wsl.mathematics.interpolator.ext.fromRatio
import com.gitee.wsl.mathematics.interpolator.ext.ratio
import com.gitee.wsl.unit.angle.AngleUnit
import com.gitee.wsl.unit.angle.toDegrees
import kotlin.math.abs

/**
 * Computes the shortest distance between this and angle, as an angle.
 *
 * @param angle the angle to measure angular distance to.
 *
 * @return the angular distance between this and `value`.
 */
fun AngleUnit.distanceTo(angle: AngleUnit): AngleUnit {
    var distance = angle.degrees - degrees
    if (distance < -180) distance += 360.0 else if (distance > 180) distance -= 360.0
    return abs(distance).toDegrees()
}

fun AngleUnit.shortDistanceTo(other: AngleUnit): AngleUnit {
    val r0 = ratio.toDouble() umod 1.0
    val r1 = other.ratio.toDouble() umod 1.0
    val diff = (r1 - r0 + 0.5) % 1.0 - 0.5
    return if (diff < -0.5) AngleUnit.fromRatio(diff + 1.0) else AngleUnit.fromRatio(diff)
}

fun AngleUnit.longDistanceTo(other: AngleUnit): AngleUnit {
    val short = shortDistanceTo(other)
    return when {
        short == AngleUnit.ZERO ->AngleUnit.ZERO
        short < AngleUnit.ZERO -> AngleUnit.FULL + short
        else -> -AngleUnit.FULL + short
    }
}